diff --git a/drivers/bios_emulator/atibios.c b/drivers/bios_emulator/atibios.c
index 3b2ed6e..93b815c 100644
--- a/drivers/bios_emulator/atibios.c
+++ b/drivers/bios_emulator/atibios.c
@@ -46,8 +46,11 @@
 *		BIOS in u-boot.
 ****************************************************************************/
 #include <common.h>
-#include "biosemui.h"
+#include <bios_emul.h>
+#include <errno.h>
 #include <malloc.h>
+#include <vbe.h>
+#include "biosemui.h"
 
 /* Length of the BIOS image */
 #define MAX_BIOSLEN	    (128 * 1024L)
@@ -59,17 +62,54 @@
 static u32 saveBaseAddress18;
 static u32 saveBaseAddress20;
 
+static void atibios_set_vesa_mode(RMREGS *regs, int vesa_mode,
+				  struct vbe_mode_info *mode_info)
+{
+	debug("VBE: Setting VESA mode %#04x\n", vesa_mode);
+	/* request linear framebuffer mode */
+	vesa_mode |= (1 << 14);
+	/* request clearing of framebuffer */
+	vesa_mode &= ~(1 << 15);
+	regs->e.eax = VESA_SET_MODE;
+	regs->e.ebx = vesa_mode;
+	BE_int86(0x10, regs, regs);
+
+	int offset = 0x2000;
+	void *buffer = (void *)(M.mem_base + offset);
+
+	u16 buffer_seg = (((unsigned long)offset) >> 4) & 0xff00;
+	u16 buffer_adr = ((unsigned long)offset) & 0xffff;
+	regs->e.eax = VESA_GET_MODE_INFO;
+	regs->e.ebx = 0;
+	regs->e.ecx = vesa_mode;
+	regs->e.edx = 0;
+	regs->e.esi = buffer_seg;
+	regs->e.edi = buffer_adr;
+	BE_int86(0x10, regs, regs);
+	memcpy(mode_info->mode_info_block, buffer,
+	       sizeof(struct vbe_mode_info));
+	mode_info->valid = true;
+
+	vesa_mode |= (1 << 14);
+	/* request clearing of framebuffer */
+	vesa_mode &= ~(1 << 15);
+	regs->e.eax = VESA_SET_MODE;
+	regs->e.ebx = vesa_mode;
+	BE_int86(0x10, regs, regs);
+}
+
 /****************************************************************************
 PARAMETERS:
 pcidev	- PCI device info for the video card on the bus to boot
-VGAInfo - BIOS emulator VGA info structure
+vga_info - BIOS emulator VGA info structure
 
 REMARKS:
 This function executes the BIOS POST code on the controller. We assume that
 at this stage the controller has its I/O and memory space enabled and
 that all other controllers are in a disabled state.
 ****************************************************************************/
-static void PCI_doBIOSPOST(pci_dev_t pcidev, BE_VGAInfo * VGAInfo)
+static void PCI_doBIOSPOST(pci_dev_t pcidev, BE_VGAInfo *vga_info,
+			   int vesa_mode, struct vbe_mode_info *mode_info)
 {
 	RMREGS regs;
 	RMSREGS sregs;
@@ -84,13 +124,16 @@
 	    ((int)PCI_DEV(pcidev) << 3) | (int)PCI_FUNC(pcidev);
 
 	/*Setup the X86 emulator for the VGA BIOS*/
-	BE_setVGA(VGAInfo);
+	BE_setVGA(vga_info);
 
 	/*Execute the BIOS POST code*/
 	BE_callRealMode(0xC000, 0x0003, &regs, &sregs);
 
 	/*Cleanup and exit*/
-	BE_getVGA(VGAInfo);
+	BE_getVGA(vga_info);
+
+	if (vesa_mode != -1)
+		atibios_set_vesa_mode(&regs, vesa_mode, mode_info);
 }
 
 /****************************************************************************
@@ -244,49 +287,109 @@
 Loads and POST's the display controllers BIOS, directly from the BIOS
 image we can extract over the PCI bus.
 ****************************************************************************/
-static int PCI_postController(pci_dev_t pcidev, BE_VGAInfo * VGAInfo)
+static int PCI_postController(pci_dev_t pcidev, uchar *bios_rom, int bios_len,
+			      BE_VGAInfo *vga_info, int vesa_mode,
+			      struct vbe_mode_info *mode_info)
 {
-	u32 BIOSImageLen;
-	uchar *mappedBIOS;
-	uchar *copyOfBIOS;
+	u32 bios_image_len;
+	uchar *mapped_bios;
+	uchar *copy_of_bios;
 
-	/*Allocate memory to store copy of BIOS from display controller*/
-	if ((mappedBIOS = PCI_mapBIOSImage(pcidev)) == NULL) {
-		printf("videoboot: Video ROM failed to map!\n");
-		return false;
+	if (bios_rom) {
+		copy_of_bios = bios_rom;
+		bios_image_len = bios_len;
+	} else {
+		/*
+		 * Allocate memory to store copy of BIOS from display
+		 * controller
+		 */
+		mapped_bios = PCI_mapBIOSImage(pcidev);
+		if (mapped_bios == NULL) {
+			printf("videoboot: Video ROM failed to map!\n");
+			return false;
+		}
+
+		bios_image_len = mapped_bios[2] * 512;
+
+		copy_of_bios = malloc(bios_image_len);
+		if (copy_of_bios == NULL) {
+			printf("videoboot: Out of memory!\n");
+			return false;
+		}
+		memcpy(copy_of_bios, mapped_bios, bios_image_len);
+		PCI_unmapBIOSImage(pcidev, mapped_bios);
 	}
 
-	BIOSImageLen = mappedBIOS[2] * 512;
-
-	if ((copyOfBIOS = malloc(BIOSImageLen)) == NULL) {
-		printf("videoboot: Out of memory!\n");
-		return false;
-	}
-	memcpy(copyOfBIOS, mappedBIOS, BIOSImageLen);
-
-	PCI_unmapBIOSImage(pcidev, mappedBIOS);
-
-	/*Save information in VGAInfo structure*/
-	VGAInfo->function = PCI_FUNC(pcidev);
-	VGAInfo->device = PCI_DEV(pcidev);
-	VGAInfo->bus = PCI_BUS(pcidev);
-	VGAInfo->pcidev = pcidev;
-	VGAInfo->BIOSImage = copyOfBIOS;
-	VGAInfo->BIOSImageLen = BIOSImageLen;
+	/*Save information in vga_info structure*/
+	vga_info->function = PCI_FUNC(pcidev);
+	vga_info->device = PCI_DEV(pcidev);
+	vga_info->bus = PCI_BUS(pcidev);
+	vga_info->pcidev = pcidev;
+	vga_info->BIOSImage = copy_of_bios;
+	vga_info->BIOSImageLen = bios_image_len;
 
 	/*Now execute the BIOS POST for the device*/
-	if (copyOfBIOS[0] != 0x55 || copyOfBIOS[1] != 0xAA) {
+	if (copy_of_bios[0] != 0x55 || copy_of_bios[1] != 0xAA) {
 		printf("videoboot: Video ROM image is invalid!\n");
 		return false;
 	}
 
-	PCI_doBIOSPOST(pcidev, VGAInfo);
+	PCI_doBIOSPOST(pcidev, vga_info, vesa_mode, mode_info);
 
 	/*Reset the size of the BIOS image to the final size*/
-	VGAInfo->BIOSImageLen = copyOfBIOS[2] * 512;
+	vga_info->BIOSImageLen = copy_of_bios[2] * 512;
 	return true;
 }
 
+int biosemu_setup(pci_dev_t pcidev, BE_VGAInfo **vga_infop)
+{
+	BE_VGAInfo *VGAInfo;
+
+	printf("videoboot: Booting PCI video card bus %d, function %d, device %d\n",
+	     PCI_BUS(pcidev), PCI_FUNC(pcidev), PCI_DEV(pcidev));
+
+	/*Initialise the x86 BIOS emulator*/
+	if ((VGAInfo = malloc(sizeof(*VGAInfo))) == NULL) {
+		printf("videoboot: Out of memory!\n");
+		return -ENOMEM;
+	}
+	memset(VGAInfo, 0, sizeof(*VGAInfo));
+	BE_init(0, 65536, VGAInfo, 0);
+	*vga_infop = VGAInfo;
+
+	return 0;
+}
+
+void biosemu_set_interrupt_handler(int intnum, int (*int_func)(void))
+{
+	X86EMU_setupIntrFunc(intnum, (X86EMU_intrFuncs)int_func);
+}
+
+int biosemu_run(pci_dev_t pcidev, uchar *bios_rom, int bios_len,
+		BE_VGAInfo *vga_info, int clean_up, int vesa_mode,
+		struct vbe_mode_info *mode_info)
+{
+	/*Post all the display controller BIOS'es*/
+	if (!PCI_postController(pcidev, bios_rom, bios_len, vga_info,
+				vesa_mode, mode_info))
+		return -EINVAL;
+
+	/*
+	 * Cleanup and exit the emulator if requested. If the BIOS emulator
+	 * is needed after booting the card, we will not call BE_exit and
+	 * leave it enabled for further use (ie: VESA driver etc).
+	*/
+	if (clean_up) {
+		BE_exit();
+		if (vga_info->BIOSImage)
+			free(vga_info->BIOSImage);
+		free(vga_info);
+		vga_info = NULL;
+	}
+
+	return 0;
+}
+
 /****************************************************************************
 PARAMETERS:
 pcidev	    - PCI device info for the video card on the bus to boot
@@ -297,38 +400,21 @@
 Boots the PCI/AGP video card on the bus using the Video ROM BIOS image
 and the X86 BIOS emulator module.
 ****************************************************************************/
-int BootVideoCardBIOS(pci_dev_t pcidev, BE_VGAInfo ** pVGAInfo, int cleanUp)
+int BootVideoCardBIOS(pci_dev_t pcidev, BE_VGAInfo **pVGAInfo, int clean_up)
 {
 	BE_VGAInfo *VGAInfo;
+	int ret;
 
-	printf("videoboot: Booting PCI video card bus %d, function %d, device %d\n",
-	     PCI_BUS(pcidev), PCI_FUNC(pcidev), PCI_DEV(pcidev));
-
-	/*Initialise the x86 BIOS emulator*/
-	if ((VGAInfo = malloc(sizeof(*VGAInfo))) == NULL) {
-		printf("videoboot: Out of memory!\n");
+	ret = biosemu_setup(pcidev, &VGAInfo);
+	if (ret)
 		return false;
-	}
-	memset(VGAInfo, 0, sizeof(*VGAInfo));
-	BE_init(0, 65536, VGAInfo, 0);
-
-	/*Post all the display controller BIOS'es*/
-	if (!PCI_postController(pcidev, VGAInfo))
+	ret = biosemu_run(pcidev, NULL, 0, VGAInfo, clean_up, -1, NULL);
+	if (ret)
 		return false;
 
-	/*Cleanup and exit the emulator if requested. If the BIOS emulator
-	is needed after booting the card, we will not call BE_exit and
-	leave it enabled for further use (ie: VESA driver etc).
-	*/
-	if (cleanUp) {
-		BE_exit();
-		if (VGAInfo->BIOSImage)
-			free(VGAInfo->BIOSImage);
-		free(VGAInfo);
-		VGAInfo = NULL;
-	}
-	/*Return VGA info pointer if the caller requested it*/
+	/* Return VGA info pointer if the caller requested it*/
 	if (pVGAInfo)
 		*pVGAInfo = VGAInfo;
+
 	return true;
 }
diff --git a/drivers/bios_emulator/besys.c b/drivers/bios_emulator/besys.c
index 70c472a..8e29a9e 100644
--- a/drivers/bios_emulator/besys.c
+++ b/drivers/bios_emulator/besys.c
@@ -713,9 +713,9 @@
 void X86API BE_outl(X86EMU_pioAddr port, u32 val)
 {
 #if !defined(CONFIG_X86EMU_RAW_IO)
-	if (IS_PCI_PORT(port))
+	if (IS_PCI_PORT(port)) {
 		PCI_outp(port, val, REG_WRITE_DWORD);
-	else if (port < 0x100) {
+	} else if (port < 0x100) {
 		DB(printf("WARN: INVALID outl.%04X <- %08X\n", (u16) port,val);)
 		LOG_outpd(port, val);
 	} else